/********************************************************************************
 Platform: SPARK V
 Experiment: 5_Velocity_Control_Using_PWM
 Written by: Vinod Desai, NEX Robotics Pvt. Ltd.
 Edited By: Sachitanand Malewar, NEX Robotics Pvt. Ltd.
 Last Modification: 22nd September 2010
 AVR Studio Version 4.17, Build 666
 
 Concepts covered: Use of timer to generate Pulse Width Modulation(PWM) for 
 velocity control
 
 This experiment demonstrates robot velocity control using PWM.
 Pulse width modulation is a process in which duty cycle of constant frequency square wave is modulated to 
 control power delivered to the load i.e. motor. 

 There are two components to the motion control:
 1. Direction control using pins PORTB0 to 	PORTB3
 2. Velocity control by PWM on pins PD4 and PD5 using OC1BL and OC1AL of timer 1.
 
 Connection Details:  	L-1---->PB0;		L-2---->PB1;
   						R-1---->PB2;		R-2---->PB3;
   						PD4 (OC1BL) ---->PWM left; 	PD5 (OC1AL) ----> PWM right;

 Make sure that motors are connected to onboard motor connectors.
 		 
 For more details refer to hardware and software manuals. 

 Note: 
 
 1. Make sure that in the configuration options following settings are 
 	done for proper operation of the code

 	Microcontroller: atmega16
 	Frequency: 7372800Hz
 	Optimization: -O0 (For more information read section: Selecting proper 
	              optimization options below figure 4.22 in the hardware manual)

*********************************************************************************/

/********************************************************************************

   Copyright (c) 2010, NEX Robotics Pvt. Ltd.                       -*- c -*-
   All rights reserved.

   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:

   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.

   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.

   * Neither the name of the copyright holders nor the names of
     contributors may be used to endorse or promote products derived
     from this software without specific prior written permission.

   * Source code can be used for academic purpose. 
	 For commercial use permission form the author needs to be taken.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE. 

  Software released under Creative Commence cc by-nc-sa licence.
  For legal information refer to: 
  http://creativecommons.org/licenses/by-nc-sa/3.0/legalcode

********************************************************************************/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

//Function to configure ports to enable robot's motion
void motion_pin_config (void)
{
 DDRB = DDRB | 0x0F;   //set direction of the PORTB3 to PORTB0 pins as output
 PORTB = PORTB & 0xF0; //set initial value of the PORTB3 to PORTB0 pins to logic 0
 DDRD = DDRD | 0x30;   //Setting PD4 and PD5 pins as output for PWM generation
 PORTD = PORTD | 0x30; //PD4 and PD5 pins are for velocity control using PWM
}

//Function to Initialize PORTs
void port_init()
{
 motion_pin_config();
}

//Timer1 is configured for constant frequency and variable duty cyclr
//TIMER1 initialize - prescale:64
// WGM: 5) PWM 8bit fast, TOP=0x00FF
// desired value: 450Hz
// actual value: 450.000Hz (0.0%)
void timer1_init(void)
{
 TCCR1B = 0x00; //stop
 TCNT1H = 0xFF; //higher byte constant frequency value of PWM cycle 
 TCNT1L = 0x01; //lower byte constant frequency value of PWM cycle 
 OCR1AH = 0x00;
 OCR1AL = 0xFF;
 OCR1BH = 0x00;
 OCR1BL = 0xFF;
 ICR1H  = 0x00;
 ICR1L  = 0xFF;
 TCCR1A = 0xA1;
 TCCR1B = 0x0D; //start Timer
}

//Function used for setting motor's direction
void motion_set (unsigned char Direction)
{
 unsigned char PortBRestore = 0;

 Direction &= 0x0F; 			// removing upper nibbel as it is not needed
 PortBRestore = PORTB; 			// reading the PORTB's original status
 PortBRestore &= 0xF0; 			// setting lower direction nibbel to 0
 PortBRestore |= Direction; 	// adding lower nibbel for direction command and restoring the PORTB status
 PORTB = PortBRestore; 			// setting the command to the port
}

void forward (void) //both wheels forward
{
  motion_set(0x06);
}

void back (void) //both wheels backward
{
  motion_set(0x09);
}

void left (void) //Left wheel backward, Right wheel forward
{
  motion_set(0x05);
}

void right (void) //Left wheel forward, Right wheel backward
{
  motion_set(0x0A);
}

void soft_left (void) //Left wheel stationary, Right wheel forward
{
 motion_set(0x04);
}

void soft_right (void) //Left wheel forward, Right wheel is stationary
{
 motion_set(0x02);
}

void soft_left_2 (void) //Left wheel backward, right wheel stationary
{
 motion_set(0x01);
}

void soft_right_2 (void) //Left wheel stationary, Right wheel backward
{
 motion_set(0x08);
}

void hard_stop (void)    //hard stop (Stop suddenly)
{
  motion_set(0x00);
}

void soft_stop (void)    //hard stop (Stop slowly)
{
  motion_set(0x0F);
}

//Function for velocity control
void velocity (unsigned char left_motor, unsigned char right_motor)
{
 OCR1AH = 0x00;
 OCR1AL = left_motor;     // duty cycle 'ON' period of PWM out for Left motor 
 OCR1BH = 0x00;
 OCR1BL = right_motor;    // duty cycle 'ON' period of PWM out for Right motor 
}

void init_devices (void)
{
 cli(); //Clears the global interrupts
 port_init();
 timer1_init();
 sei(); //Enables the global interrupts
}

//Main Function
int main(void)
{
 //unsigned char i;
 
 init_devices();

 velocity (35, 35); //Set robot velocity here. Smaller the value lesser will be the velocity
					//Try different valuse between 0 to 255
 while(1)
 {
 /*  
 	Use this code segment to graduali increace robot velocity in sawtooth pattern, comment the bottom code segment
	
	forward(); //both wheels forward
	for(i = 0;i<255;i++)
    {
	velocity (i, i); //Set robot velocity here. More value will result in more velocity
	
	_delay_ms(100);
    }
 */	     
	forward(); //both wheels forward
	_delay_ms(1000);

	hard_stop();						
	_delay_ms(500);

	back(); //both wheels backward						
	_delay_ms(1000);

	hard_stop();						
	_delay_ms(500);
	
	left(); //Left wheel backward, Right wheel forward
	_delay_ms(500);
	
	hard_stop();						
	_delay_ms(500);
	
	right(); //Left wheel forward, Right wheel backward
	_delay_ms(500);

	hard_stop();						
	_delay_ms(500);

	soft_left(); //Left wheel stationary, Right wheel forward
	_delay_ms(500);
	
	hard_stop();						
	_delay_ms(500);

	soft_right(); //Left wheel forward, Right wheel is stationary
	_delay_ms(500);

	hard_stop();						
	_delay_ms(500);

	soft_left_2(); //Left wheel backward, right wheel stationary
	_delay_ms(500);

	hard_stop();						
	_delay_ms(500);

	soft_right_2(); //Left wheel stationary, Right wheel backward
	_delay_ms(500);

	hard_stop();						
	_delay_ms(500);
 }
}
